% Adjoint-Control-Transformation
%
% Input arguments:
% -------------------------------------------------------------------------
% alpha, dalpha, beta, dbeta : initial guess of angle and angle rate
% S : initial guess of switching function
% dS : initial guess of fir
% m : initial mass
% pm : lambda of mass at t0
% auxdata : parameters
% 
% Output arguments:
% -------------------------------------------------------------------------
% p0 : initial guess of co-states
%
% -----------------------------------------------------------------------
% Copyright (C) 09, Nov, 2013 by Zhang Chen, chenzhang.buaa@gmail.com
% -----------------------------------------------------------------------

function [p0] = act(alpha , dalpha , beta , dbeta , S , dS , m , pm , auxdata)

c = auxdata.c;
mu = auxdata.mu;
Tmax = auxdata.Tmax;

r = auxdata.x0(1:3);
v = auxdata.x0(4:6);

% ------------------------------------------------------------------
d2 = (r(1) + mu)^2 + r(2)^2 + r(3)^2;
r2 = (r(1) - (1 - mu))^2 + r(2)^2 + r(3)^2;
d3= d2^1.5;
r3= r2^1.5;

Ux = r(1)  - (1 - mu) * (r(1) + mu) / d3 - mu * (r(1) - (1 - mu)) / r3;
Uy = r(2) - (1 - mu) * r(2) / d3 - mu*r(2) / r3;
Uz = -(1 - mu) * r(3) / d3 - mu*r(3) / r3;
dv = [Ux + 2*v(2) ;
    Uy - 2*v(1) ;
    Uz];

% ------------------------------------------------------------------
u_prime = [cos(alpha) * cos(beta);
    sin(alpha) * cos(beta);
    sin(beta)];

du_prime = [-sin(alpha) * dalpha * cos(beta) - cos(alpha) * sin(beta) * dbeta;
    cos(alpha) * dalpha * cos(beta) - sin(alpha) * sin(beta) * dbeta;
    cos(beta) * dbeta];

% ------------------------------------------------------------------
w = cross(r , v);
dw = cross(r , dv);
w_mag = sqrt(w.' * w);
dw_mag = w.' * dw / w_mag;
dw_unit = dw / w_mag - w * dw_mag / w_mag^2;

v_mag = sqrt(v.' * v);
dv_mag = v.' * dv / v_mag;
dv_unit = dv / v_mag - v * dv_mag / v_mag^2;

v_unit = v / v_mag;
w_unit = w / w_mag;

R = [v_unit , cross(w_unit , v_unit) , w_unit];
dR = [dv_unit , cross(dw_unit , v_unit) + cross(w_unit , dv_unit) , dw_unit];

u = R * u_prime;
du = dR * u_prime + R * du_prime;

% ------------------------------------------------------------------
% for 3-body problem
H = [0 , 2 , 0;
    -2 , 0 , 0;
    0 , 0 , 0];

epsilon = 1;
if S < -epsilon;
    u_io = 1;
elseif S >= -epsilon && S <= epsilon;
    u_io = 0.5 * (epsilon - S) / epsilon;
else % S > epsilon;
    u_io = 0;
end

% ------------------------------------------------------------------
pv_mag = m / c * (-S - pm + 1); 
dpv_mag = -Tmax * u_io / c^2 * (-S - pm + 1) + m / c * (-dS + pv_mag * Tmax * u_io / m^2);

pv = pv_mag * u;
dpv = dpv_mag * u + pv_mag * du; 
pr = -dpv - H' * pv;

% ------------------------------------------------------------------
p0 = [pr ; pv ; pm];

end